#include <webots/Supervisor.hpp>

#include <iostream>

using namespace webots;

//the following are UBUNTU/LINUX ONLY terminal color codes.
#define RESET   "\033[0m"
#define BLACK   "\033[30m"      /* Black */
#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define YELLOW  "\033[33m"      /* Yellow */
#define BLUE    "\033[34m"      /* Blue */
#define MAGENTA "\033[35m"      /* Magenta */
#define CYAN    "\033[36m"      /* Cyan */
#define WHITE   "\033[37m"      /* White */
#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */

class ToLSupervisor : public Supervisor {
    
public:
    
    ToLSupervisor() : Supervisor() {
        
    }
    
    virtual ~ToLSupervisor() {
    }
    
    void run() {
        double timeStep = getBasicTimeStep();
        
        std::cout << BOLDBLUE << "Starting Supervisor, running experiment untill notified " << RESET << std::endl;
        
        receiver = getReceiver("receiver");
        receiver->enable(timeStep);
        
        while (step(timeStep) != -1) {
            while(receiver->getQueueLength() > 0){
                std::string message((char*)receiver->getData());
                if(message == "simulationdone"){
                    std::cout << BOLDBLUE << "Received end notification, reverting simulation!" << RESET << std::endl;
                    simulationRevert();
                }
                receiver->nextPacket();
            }
        }
    }
    
private:
    Receiver *receiver;
};

int main(int argc, char **argv)
{
    ToLSupervisor* controller = new ToLSupervisor();
    controller->run();
    delete controller;
    return 0;
}
